#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include "Components/ATK_MS901M/atk_ms901m.h"
#include "Components/simple_json.h"
#include <ti/driverlib/m0p/dl_interrupt.h>

#define TAG "Main"
extern void test_patrol_pool_with_sys_delta(uint16_t *delta_sys_tick);
extern void motion_pid_rst(float kp, float ki, float kd, float m);


int main(void)
{
  SYSCFG_DL_init();
  SysTick_Init();
  uart_init();
  OLED_Init();
  motor_init();
  motion_pid_rst(1.2, 0, 10.1, 50);

  OLED_ShowString(0, 0, "Tick Test", 16);

  unsigned long cur_sys_tick = 0;
  unsigned long last_sys_tick = 0;
  uint16_t delta_sys_tick = 0;

  mspm0_get_clock_ms(&last_sys_tick);
  while (1)
  {
    mspm0_get_clock_ms(&cur_sys_tick);
    delta_sys_tick = cur_sys_tick - last_sys_tick;
    last_sys_tick = cur_sys_tick;

    test_patrol_pool_with_sys_delta(&delta_sys_tick);

    // log(TAG, "delta_sys_tick: %d", delta_sys_tick);
    mspm0_delay_us(10000);
  }
}
